#include "../include/wirelessimage_control/wirelessimage_control_node.h"
// #include "../include/wirelessimage_control/df_fifo.h"

namespace CAR_CONTROL
{



carControl::carControl() :frist_sn_(false), serial_timeout_(20),faultSymbol_flag(0)
{
  ros::NodeHandle pravite_nh("~");
  //topic_name & frame_id
  pravite_nh.param("debug",     if_debug_,  false);
  pravite_nh.param("car_topic",car_topic_,std::string("/cmd_vel"));
  pravite_nh.param("base_frame", base_frame_id_, std::string("base_footprint")); 
  //serial                                                 
  pravite_nh.param("port", serial_port_, std::string("/dev/carSBUS")); 
  pravite_nh.param("baud", serial_baud_, 115200);
  //publisher
  car_pub_=nh_.advertise<geometry_msgs::Twist>("/raw_cmd_vel",10);
  //subscribe 接受上层下发的速度指令
  // cmd_sub_ = nh_.subscribe<geometry_msgs::Twist>(car_topic_.c_str(),1,&carControl::cmd_vel_callback,this);      

  // 设置速度发布定时器 timer1每50ms触发一次callback1函数
  // ros::Time::init(); 
   
  //setp up serial
  // printf("---------------------------------------------\r\n");
  try
  {
   
    serial_.setPort(serial_port_);
       printf("---------------------------------------------\r\n");
    serial_.setBaudrate(serial_baud_);
    serial_.setFlowcontrol(serial::flowcontrol_none);
            
    serial_.setParity(serial::parity_none); //default is parity_none
    serial_.setStopbits(serial::stopbits_one);
    serial_.setBytesize(serial::eightbits);
    serial::Timeout time_out = serial::Timeout::simpleTimeout(serial_timeout_);
    serial_.setTimeout(time_out);

    serial_.open();

  }
  catch (serial::IOException &e)
  {
    ROS_ERROR_STREAM("Unable to open port ");
    exit(0);
  }
  if (serial_.isOpen())
  {
    ROS_INFO_STREAM("Serial Port initialized");
  }
  else
  {
    ROS_ERROR_STREAM("Unable to initial Serial port ");
    exit(0);
  }
  ros::Timer send_speed_timer = nh_.createTimer(ros::Duration(0.01),&carControl::send_speed_callback,this); 
  
  
  boost::thread recv_thread(boost::bind(&carControl::receive_func,this));     

      memset(buffer, 0, 250 * sizeof(uint8_t));//初始化缓冲区
  //     memset(test, 0, 100*sizeof(uint8_t));
  //     for (int i = 0; i < 100; i++)
  //         data[i] = i;
      Df_FifoReset(&Fifostructdefine, buffer, 250);//初始化缓冲区结构
  // #if debug
  //     printf("The buffer count is %d\n", Fifostructdefine.Count);
  //     printf("The buffer length is %d\n", Fifostructdefine.length);
  //     printf("The buffer Pread is %d\n", Fifostructdefine.read);
  //     printf("The buffer Pwrite is %d\n", Fifostructdefine.write);
  // #endif // debug
  //     printf(">>>>>>>>>>>>>>>>>>>>>>>>>>>>>写入测试<<<<<<<<<<<<<<<<<<<<<<<\n");
  //     Df_FifoInput(&Fifostructdefine, data, 100);
  // #if debug
  //     printf("The buffer count is %d\n", Fifostructdefine.Count);
  //     printf("The buffer length is %d\n", Fifostructdefine.length);
  //     printf("The buffer Pread is %d\n", Fifostructdefine.read);
  //     printf("The buffer Pwrite is %d\n", Fifostructdefine.write);
  // #endif // debug
  //     /*直接读取测试*/
  //     printf(">>>>>>>>>>>>>>>>>>>>>>>>>直接读取测试<<<<<<<<<<<<<<<<<<<<<<<\n");
  //     Df_FifoOutput(&Fifostructdefine, test, 50);
  //     for (int i = 1;i <51;i++)
  //     {
  //         printf("test[%d] = %d\t", i-1, test[i-1]);
  //         if (i % 5 == 0)
  //         {
  //             printf("\n");
  //         }
  //     }
  // #if debug
  //     printf("The buffer count is %d\n", Fifostructdefine.Count);
  //     printf("The buffer length is %d\n", Fifostructdefine.length);
  //     printf("The buffer Pread is %d\n", Fifostructdefine.read);
  //     printf("The buffer Pwrite is %d\n", Fifostructdefine.write);
  // #endif // debug
  //     /*擦除测试*/
  //     printf(">>>>>>>>>>>>>>>>>>>>>擦除测试<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<\n");
  //     Df_FifoErase(&Fifostructdefine, 30);
  // #if debug
  //     printf("The buffer count is %d\n", Fifostructdefine.Count);
  //     printf("The buffer length is %d\n", Fifostructdefine.length);
  //     printf("The buffer Pread is %d\n", Fifostructdefine.read);
  //     printf("The buffer Pwrite is %d\n", Fifostructdefine.write);
  // #endif // debug
  //     /*读取擦除测试*/
  //     printf(">>>>>>>>>>>>>>>>>>>>>读取擦除测试<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<\n");
  //     while (Df_FifoOutEaser(&Fifostructdefine,&data[0],1) == Df_StateOk)
  //     {
  //         printf("Df_FifoOutEaser data is  %d\n", data[0]);
  //     }
  // #if debug
  //     printf("The buffer count is %d\n", Fifostructdefine.Count);
  //     printf("The buffer length is %d\n", Fifostructdefine.length);
  //     printf("The buffer Pread is %d\n", Fifostructdefine.read);
  //     printf("The buffer Pwrite is %d\n", Fifostructdefine.write);
  // #endif // debug
  //     system("pause");
    
  
  ros::spin(); //开启回调循环
  // processLoop();

}

carControl::~carControl()
{
  boost::mutex::scoped_lock look(mutex_);
  if (serial_.isOpen())
    serial_.close();
}
//辅助函数，计算串口通信校验位
void carControl::check_sum(uint8_t* data, size_t len, uint8_t& dest)
{
    dest = 0x00;
    for(int i=0;i<len;i++)
    {
        dest += *(data + i);
    }
}
int carControl::write_faultSymbolState(faultSymbolState bit)
{
    faultSymbol_flag |= (1<<bit) ;
    return faultSymbol_flag;
}
//健康函数
// void carControl::regularHealthExamination()
// {


// }
//  /cmd_vel速度话题回调函数
void carControl::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
{
    try
    {
        cmd_vel_mutex_.lock();
        last_twist_time_ = ros::Time::now();
        current_twist_ = *msg.get();
        cmd_vel_mutex_.unlock();
    }
    catch(...)
    {
        cmd_vel_mutex_.unlock();
    }
}
//对cmd_vel数据进行处理
void carControl::cmd_data_processing(double& x,double& z, uint8_t& mode)
{
  double x_w, z_w;
  if((x!=0)&&(z!=0)){
    mode = error_default;
  }
  if(mode==startMode){
    x_w= 0.08;
    z_w = 0.21;
  }
  else if((mode > startMode)&&(mode <= timingMode)){
    x_w= 0.06;
    z_w = 0.19;
  }
  else{
    x_w= 0;
    z_w = 0;
    x =0;
    z = 0;
  }
    if((x > 0)&&(x <= 1)){
      x = round((1684-1024)*x_w)+1024;  
    }
    else if(x==0){
      x = 1024;  
    }
    else if((x<0)&&(x>=-1)){
      x_w = (-x_w);
      x = round((1024-364)*x_w)+1024;
    }
    else{
       x = 1024;    
    }   
    if((z > 0)&&(z <= 1)){
      z = round((1024-364)*z_w)+1024;
    }
    else if(z==0){
      z = 1024;
    }
    else if((z<0)&&(z>=-1)){
      z_w = -z_w;
      z = round((1684-1024)*z_w)+1024;
    } 
    else{
      z = 1024;  
    }

    
    //将当前标志记录下来，以便下次使用
    if(x !=1024 )   
      x_last = 1; 
    else
      x_last = 0;  
    
    if(z != 1024)
      z_last = 1;  
    else
      z_last = 0;   


}


//启动，停止，运行模式处理
void carControl::Pattern_handler(double& x,double& z)
{
    uint8_t test = error_default;
   
    switch (car_mode)
    {
    case startMode:
      if((x_last==0)&&(z_last ==0)&&((z!=0)||(x!=0)))   //直行模式 转弯模式
      {
        test = car_mode;
        car_mode = timingMode;
        running_count = ros::Time::now();
        ROS_DEBUG_STREAM("CMD_VEL OK!!!!\r\n");
      }
      else if((x!=0)&&(z!=0))
      {
        ROS_DEBUG_STREAM("CMD_VEL ERROR!!!!\r\n");
        test = error_default;
      }
      else
      {
        ROS_DEBUG_STREAM("CMD_VEL call not data!!!!\r\n");
        test = error_default;
      }
      break;
    case stopMode:
      test = error_default;
      if((x!=0)||(z!=0))
      {
        car_mode = startMode;
      }
      ROS_DEBUG_STREAM("stopMode !!!!\r\n");
      break;
    case runningMode:
      break;
    case transitionMode:
      if((x_last==1)&&(z != 0))   //直行变转弯
      {
        ROS_DEBUG_STREAM("直行变转弯 !!!!\r\n");
      }
      else if((z_last==1)&&(x != 0))   //转弯变直行
      {
        ROS_DEBUG_STREAM("转弯变直行 !!!!\r\n");
      }
      else if((x_last==1)&&(x != 0))  //继续直行
      {
        ROS_DEBUG_STREAM("继续直行 !!!!\r\n");
      }
      else if((z_last==1)&&(z != 0))  //继续转弯
      {
        ROS_DEBUG_STREAM("继续转弯 !!!!\r\n");
      }
      else{                           //停止状态
        car_mode = stopMode;
        printf("============================\r\n");
      }
      test = transitionMode;
      ROS_DEBUG_STREAM("transitionMode !!!!\r\n");
        break;
    case timingMode:
      if((x==0)&&(z==0))
      {
        car_mode = stopMode;
      }
      if((ros::Time::now() - running_count).toSec() > 0.5)
      {
        car_mode = transitionMode;
      }

      test = startMode;
      ROS_DEBUG_STREAM("timingMode !!!!\r\n");
      break;
    default:
      test = error_default;
      ROS_DEBUG_STREAM("default !!!!\r\n");
      break;
    }
      cmd_data_processing(x, z, test);
}
void carControl::send_speed_callback(const ros::TimerEvent&)
{
     if (Df_FifoOutEaser(&Fifostructdefine,&data[0],25) == Df_StateOk)
      {
            //处理判断结果
              RemoteControlChannel[0] = ((data[2] << 8) + data[1]) >> 0x00 & 0x07FF;                                        
              RemoteControlChannel[1] = ((data[3] << 8) + data[2]) >> 0x03 & 0x07FF;                                        
              RemoteControlChannel[2] =  ((data[5] & 0x01) << 10) + ((data[4] << 8) + data[3] >> 0x06);     
              //RemoteControlChannel[2] = ((data[5] << 16) + (data[4] << 8) + data[3]) >> 0x06 & 0x07FF;    
              RemoteControlChannel[3] = ((data[6] << 8) + data[5]) >> 0x01 & 0x07FF;                                        
              RemoteControlChannel[4] = ((data[7] << 8) + data[6]) >> 0x04 & 0x07FF;                                        
              RemoteControlChannel[5] =  ((data[9] & 0x03) << 9) + ((data[8] << 8) + data[7] >> 0x07);      
              //RemoteControlChannel[5] = ((data[9] << 16) + (data[8] << 8) + data[7]) >> 0x07 & 0x07FF;    
              RemoteControlChannel[6] = ((data[10] << 8) + data[9]) >> 0x02 & 0x07FF;                                       
              RemoteControlChannel[7] = ((data[11] << 8) + data[10]) >> 0x05 & 0x07FF;                                      
              RemoteControlChannel[8] = ((data[13] << 8) + data[12]) >> 0x00 & 0x07FF;                                      
              RemoteControlChannel[9] = ((data[14] << 8) + data[13]) >> 0x03 & 0x07FF;                                      
              //RemoteControlChannel[10] = ((data[16] << 16) + (data[15] << 8) + data[14]) >> 0x06 & 0x07FF;
              RemoteControlChannel[10] =  ((data[16] & 0x01) << 10) + ((data[15] << 8) + data[14] >> 0x06); 
              RemoteControlChannel[11] = ((data[17] << 8) + data[16]) >> 0x01 & 0x07FF;                                     
              RemoteControlChannel[12] = ((data[18] << 8) + data[17]) >> 0x04 & 0x07FF;                                     
              //RemoteControlChannel[13] = ((data[20] << 16) + (data[19] << 8) + data[18]) >> 0x07 & 0x07FF;
              RemoteControlChannel[13] =  ((data[20] & 0x03) << 9) + ((data[19] << 8) + data[18] >> 0x07);  
              RemoteControlChannel[14] = ((data[21] << 8) + data[20]) >> 0x02 & 0x07FF;                                     
              RemoteControlChannel[15] = ((data[22] << 8) + data[21]) >> 0x05 & 0x07FF;                                     
              RemoteControlChannel[16] = data[23]; 

            //   for (int i = 1;i <17;i++)
            // {
            //     printf("RemoteControlChannel[%d] = %d\t", i-1, RemoteControlChannel[i-1]);
            //     if (i % 5 == 0)
            //     {
            //         printf("\n");
            //     }
            // }
               if((RemoteControlChannel[3]<=1722)&&(RemoteControlChannel[3]>=282)&&(RemoteControlChannel[1]<=1722)&&(RemoteControlChannel[1]>=282)){
                angular_speed = -(int(RemoteControlChannel[3]-MEDIAN)/STAT_VEL);
                linear_speed = -(int(RemoteControlChannel[1]-MEDIAN)/STAT_VEL);
                current_twist_.linear.x = linear_speed;
                current_twist_.angular.z = angular_speed;
               
              }
      }
      else{
        // linear_speed = 0;
        // angular_speed = 0;
        // printf("mei you shu ju  \r\n");
      }
    // if((ros::Time::now() - last_twist_time_).toSec() <= 1.0)
    // {
    //     linear_speed = current_twist_.linear.x;
    //     angular_speed = -current_twist_.angular.z;
    //     // printf("linear_speed: %lf  angular_speed:  %lf \r\n",linear_speed,angular_speed);
    // }
    // else
    // {
    //     linear_speed = 0.0;
    //     angular_speed = 0.0;
    // }
      //记录当前的时间
      now_ = ros::Time::now();
      //判断接收是否超时
      if((now_ - last_time_).toSec() > 2.0)
      {
        //通讯故障标志，请处理
        // write_faultSymbolState(receiveTimeout);
        current_twist_.linear.x = 0;
        current_twist_.angular.z = 0;
        ROS_DEBUG_STREAM("communication failure\r\n");
      }
    // linear = (int)(linear_speed);
    // angular = (int)(angular_speed);
    // printf("linear: %lf  angular:  %lf \r\n",linear_speed,angular_speed);
    car_pub_.publish(current_twist_);
    // ROS_DEBUG_STREAM("send angular_speed -> left: " << angular_speed <<"; linear_speed: " << linear_speed);

}
//用于接收car的数据，判断通信是否良好
void carControl::receive_func()
{
    uint8_t payload_size, check_num, buffer_data[255],payload_type;
    
    state_ = waitingForHead;
    size_t type_s;
    bool recv_flag_ = true;
    last_time_ = ros::Time::now();
    while(recv_flag_)
    {
      //记录当前的时间
      now_ = ros::Time::now();
      //判断接收是否超时
      if((now_ - last_time_).toSec() > 1.0)
      {
        //通讯故障标志，请处理
        write_faultSymbolState(receiveTimeout);
        ROS_DEBUG_STREAM("communication failure\r\n");
      }
        switch (state_)
        {
            case waitingForHead:
                check_num = 0x00;       
                type_s = serial_.read(buffer_data, 1);
                state_ = buffer_data[0] == head ? waitingForPayload : waitingForHead;
                if(state_ == waitingForHead)
                {
                    ROS_DEBUG_STREAM("recv head error : ->"<<(int)buffer_data[0]);
                }
                break;
            case waitingForPayload:
                type_s = serial_.read(&buffer_data[1], 24);
                state_ = waitingForCheckSum;
                break;
            case waitingForCheckSum:
                // check_sum(buffer_data,19,check_num);
                state_ = ((buffer_data[23] == 0)&&(buffer_data[24==0])) ? handlePayload : waitingForHead;
                if(state_ == waitingForHead)
                {
                    ROS_DEBUG_STREAM("check sum error! recv is  : ->"<<(int)buffer_data[19]<<"  calc is "<<check_num);
                }
                break;
            case handlePayload:
              //写入FIFO
                Df_FifoInput(&Fifostructdefine, buffer_data, 25);
                //处理判断结果
              // RemoteControlChannel[0] = ((buffer_data[2] << 8) + buffer_data[1]) >> 0x00 & 0x07FF;                                        
              // RemoteControlChannel[1] = ((buffer_data[3] << 8) + buffer_data[2]) >> 0x03 & 0x07FF;                                        
              // RemoteControlChannel[2] =  ((buffer_data[5] & 0x01) << 10) + ((buffer_data[4] << 8) + buffer_data[3] >> 0x06);     
              // //RemoteControlChannel[2] = ((buffer_data[5] << 16) + (buffer_data[4] << 8) + buffer_data[3]) >> 0x06 & 0x07FF;    
              // RemoteControlChannel[3] = ((buffer_data[6] << 8) + buffer_data[5]) >> 0x01 & 0x07FF;                                        
              // RemoteControlChannel[4] = ((buffer_data[7] << 8) + buffer_data[6]) >> 0x04 & 0x07FF;                                        
              // RemoteControlChannel[5] =  ((buffer_data[9] & 0x03) << 9) + ((buffer_data[8] << 8) + buffer_data[7] >> 0x07);      
              // //RemoteControlChannel[5] = ((buffer_data[9] << 16) + (buffer_data[8] << 8) + buffer_data[7]) >> 0x07 & 0x07FF;    
              // RemoteControlChannel[6] = ((buffer_data[10] << 8) + buffer_data[9]) >> 0x02 & 0x07FF;                                       
              // RemoteControlChannel[7] = ((buffer_data[11] << 8) + buffer_data[10]) >> 0x05 & 0x07FF;                                      
              // RemoteControlChannel[8] = ((buffer_data[13] << 8) + buffer_data[12]) >> 0x00 & 0x07FF;                                      
              // RemoteControlChannel[9] = ((buffer_data[14] << 8) + buffer_data[13]) >> 0x03 & 0x07FF;                                      
              // //RemoteControlChannel[10] = ((buffer_data[16] << 16) + (buffer_data[15] << 8) + buffer_data[14]) >> 0x06 & 0x07FF;
              // RemoteControlChannel[10] =  ((buffer_data[16] & 0x01) << 10) + ((buffer_data[15] << 8) + buffer_data[14] >> 0x06); 
              // RemoteControlChannel[11] = ((buffer_data[17] << 8) + buffer_data[16]) >> 0x01 & 0x07FF;                                     
              // RemoteControlChannel[12] = ((buffer_data[18] << 8) + buffer_data[17]) >> 0x04 & 0x07FF;                                     
              // //RemoteControlChannel[13] = ((buffer_data[20] << 16) + (buffer_data[19] << 8) + buffer_data[18]) >> 0x07 & 0x07FF;
              // RemoteControlChannel[13] =  ((buffer_data[20] & 0x03) << 9) + ((buffer_data[19] << 8) + buffer_data[18] >> 0x07);  
              // RemoteControlChannel[14] = ((buffer_data[21] << 8) + buffer_data[20]) >> 0x02 & 0x07FF;                                     
              // RemoteControlChannel[15] = ((buffer_data[22] << 8) + buffer_data[21]) >> 0x05 & 0x07FF;                                     
              // RemoteControlChannel[16] = buffer_data[23]; 
             

            // for (int i = 1;i <17;i++)
            // {
            //     printf("RemoteControlChannel[%d] = %d\t", i-1, RemoteControlChannel[i-1]);
            //     if (i % 5 == 0)
            //     {
            //         printf("\n");
            //     }
            // }
                
                last_time_ = ros::Time::now();
                state_ = waitingForHead;
                break;
        
            default:
                state_ = waitingForHead;
                break;
        }
    }

}
void carControl::processLoop()
{
  ROS_INFO("carControl::processLoop: start");
  while (ros::ok())
  {
    if (!serial_.isOpen())
    {
      ROS_WARN("serial unopen");
    } 

  }

}




} //namespace CAR_CONTROL

int main(int argc, char **argv)
{
  ros::init(argc, argv, "car_control");
  CAR_CONTROL::carControl bp;

  return 0;
}
